import sensor, image, time
from pyb import UART

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.skip_frames(time=2000)

# 初始化串口（UART3，波特率115200）
uart = UART(3, 115200)  # OpenMV4P的UART3对应P4(TX)/P5(RX)

# 颜色阈值配置（LAB格式）
COLOR_THRESHOLDS = [
    ("Red",   (30, 100,  20, 60,  -20, 20),  (255, 0, 0)),
    ("Blue",  (50, 90,  -40, -10, -30, 30),  (0, 0, 255)),
    ("White", (70, 100, -10, 10,  -10, 10),  (255, 255, 255)),
    ("Black", (20, 60,  -20, 20,  -30, 10),  (0, 0, 0)),
    ("Yellow",(60, 100, -20, 20,   20, 60),  (255, 255, 0)),
    ("Green", (40, 80,  -60, -20,  10, 40),  (0, 255, 0))
]

# 车辆筛选参数
MIN_AREA = 100
MAX_AREA = 10000
MIN_ASPECT_RATIO = 1.2
MAX_ASPECT_RATIO = 4.0

def is_car(blob):
    aspect_ratio = blob.w() / blob.h() if blob.h() != 0 else 0
    return (MIN_AREA < blob.area() < MAX_AREA and 
            MIN_ASPECT_RATIO < aspect_ratio < MAX_ASPECT_RATIO)

while True:
    img = sensor.snapshot()
    car_count = 0

    for color_name, thresholds, box_color in COLOR_THRESHOLDS:
        blobs = img.find_blobs([thresholds], 
                               pixels_threshold=500, 
                               merge=True, 
                               margin=10)
        for blob in blobs:
            if is_car(blob):
                car_count += 1
                img.draw_rectangle(blob.rect(), color=box_color)
                img.draw_string(blob.x(), blob.y()-10, 
                                color_name, color=box_color)

    img.draw_string(0, 0, f"cars: {car_count}", color=(255, 0, 0))
    
    # 发送数据前将字符串编码为字节
    uart.write("car1={},car2=30\r\n".format(car_count).encode())  # 关键修复点！
    
    time.sleep_ms(100)